#ifndef KINGEMATICS_H
#define KINGEMATICS_H

#include "Arduino.h"

/*
    1.运动学正逆解(两个轮子的实时速度->当前实时的角度速度和线速度 / 当前目标的角速度和线速度->两个轮子的目标速度)
*/

typedef struct{
    float per_pulse_distance;   //每个脉冲的前进的距离
    int16_t motor_speed;        //单位用mm/s
    int64_t last_encoder_ticks; //上次电机编码器的读数

}motor_param_t;

struct odom_t{

    float x;
    float y;
    float angle;
    float linear_speed;
    float angular_spedd;
};

class Kinematics
{

private:
    motor_param_t motor_param[2];
    uint64_t last_update_time = 0;  //用于存储上一次更新电机速度的时间,计算速度的时候使用
    float wheel_distance = 0.0;     //两个轮子之间的距离
    odom_t odom;                    //用于存储历程计信息

public:
    Kinematics()=default;
    ~Kinematics()=default;
    void set_motor_param(uint8_t id,float per_pluse_distance);

    //运动学正解,将左右轮的速度转换成线速度和角速度
    void kinematics_forward(float left_speed,float right_speed,float*out_liner_speed,float*out_anglar_speed);

    //运动学逆解,将线速度和角速度转换成左右轮的速度
    void kinematics_inverse(float linear_speed,float angular_speed,float*out_left_speed,float*out_right_speed);

    //更新电机速度和编码器数据
    void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick);

    //获取电机速度,返回值是速度
    int16_t get_motor_speed(uint8_t id);

    //设置轮子的间距
    void set_wheel_distance(float distance);

    //更新历程计函数
    void update_odom(int dt);
    odom_t & get_odom();
    void TransAngleInPI(float angle,float & out_angle); 
};



#endif //KINGEMATICS_H